#include "BPECollisionWorld.h"
#include <queue>
#include <irrlicht.h>
using namespace irr;
using namespace core;
using namespace std;
BPECollisionWorld::BPECollisionWorld()
	:bpeAreaCubes(boost::extents[1][1][1]){
	xArraySize = 1;
	yArraySize = 1;
	zArraySize = 1;
	
	//i have to add a cube
	bpeAreaCubes[0][0][0] = shared_ptr<BPECollisionWorldArea>(new BPECollisionWorldArea(BPEPoint3d(0,0,0)));

};


BPECollisionWorld::BPECollisionWorld(BPEPoint3d worldEdge1, BPEPoint3d worldEdge2)
	:bpeAreaCubes(boost::extents
		[(int)((worldEdge2 - worldEdge1).getX()/cubeLineLength)+1]
		[(int)((worldEdge2 - worldEdge1).getY()/cubeLineLength)+1]
		[(int)((worldEdge2 - worldEdge1).getZ()/cubeLineLength)+1])
		{
	

	xArraySize = (int)((worldEdge2 - worldEdge1).getX()/cubeLineLength)+1;
	yArraySize = (int)((worldEdge2 - worldEdge1).getY()/cubeLineLength)+1;
	zArraySize = (int)((worldEdge2 - worldEdge1).getZ()/cubeLineLength)+1;


	//i have to add cubes
	for(int x = 0; x < xArraySize; x++)
		for(int y = 0; y < yArraySize; y++)
			for(int z = 0; z < zArraySize; z++)
				bpeAreaCubes[x][y][z] = shared_ptr<BPECollisionWorldArea>(new BPECollisionWorldArea(BPEPoint3d(x,y,z)));
	
};


void BPECollisionWorld::addBoundingShape(shared_ptr<BPEBoundingShape> &bShape){

	BPEBoundingBox* temp = (BPEBoundingBox*)bShape.get();
	BPEPoint3d tempVec = temp->getBoundingBox().getCenter();
	
	//find the area code of the center
	BPEPoint3d tempPoint = getAreaCode( tempVec	 );

	bShape->setAreaCodes(tempPoint.getX(), tempPoint.getY(), tempPoint.getZ());

	int code = bpeBoundShapes.size();
	bpeAreaCubes[tempPoint.getX()][tempPoint.getY()][tempPoint.getZ()]->addCollisionShapeCode(code);
	bpeBoundShapes.push_back(bShape); 
};
BPEPoint3d BPECollisionWorld::getAreaCode(BPEPoint3d p){

	BPEPoint3d areaCode;

	areaCode.setX(	(int)(p.getX() / cubeLineLength)	);
	areaCode.setY(	(int)(p.getY() / cubeLineLength)	);
	areaCode.setZ(	(int)(p.getZ() / cubeLineLength)	);

	if(areaCode.getX()<0 || areaCode.getY()<0 || areaCode.getZ()<0){
		areaCode = BPEPoint3d(0,0,0);
	}

	if(areaCode.getX() >= xArraySize)
		areaCode.setX(xArraySize-1);

	if(areaCode.getY() >= yArraySize)
		areaCode.setY(yArraySize-1);

	if(areaCode.getZ() >= zArraySize)
		areaCode.setZ(zArraySize-1);

	return areaCode;
	
};

void BPECollisionWorld::collisionRoutine(vector<bool> onMove, vector<BPEVector3d> speeds, vector<BPECollisionData> &collisionPairs, vector<BPEVector3d> &fixSpeeds){

	recalculateAreaCodes(onMove);

	getCollisionPairs(collisionPairs);
  
  //fill the fixSpeeds vector with zero magnitude vectors to avoid mistakes
  
  fixSpeeds.assign(speeds.size(),BPEVector3d(0,0,0));
  
  //correct all the collisions
  for(vector<BPECollisionData>::size_type i= 0; i < collisionPairs.size(); i++){
    
    
    shared_ptr<BPEBoundingShape> firstBound = bpeBoundShapes[collisionPairs[i].firstObjectCode];
		shared_ptr<BPEBoundingShape> secondBound = bpeBoundShapes[collisionPairs[i].secondObjectCode];

    BPEVector3d firstSpeed = speeds[collisionPairs[i].firstObjectCode];
    BPEVector3d secondSpeed = speeds[collisionPairs[i].secondObjectCode];

		firstSpeed.inverse();
		secondSpeed.inverse();
    
    if(firstSpeed.getMagnitude() > 0)
      firstSpeed.setMagnitude(firstSpeed.getMagnitude() * 0.1);
     if(secondSpeed.getMagnitude() > 0)
       secondSpeed.setMagnitude(secondSpeed.getMagnitude() * 0.1);
    
    
    fixSpeeds[collisionPairs[i].firstObjectCode] = firstSpeed;
    fixSpeeds[collisionPairs[i].secondObjectCode] = secondSpeed;


    double x;
    while(firstBound->collide(secondBound.get(),BPEVector3d(),x)){
      
      if(firstSpeed.getMagnitude() > 0 && secondSpeed.getMagnitude() > 0 ){
        
        firstBound->updatePositionAndRotation(firstBound->getCenter().add(firstSpeed).getHead());
        secondBound->updatePositionAndRotation(secondBound->getCenter().add(secondSpeed).getHead());

        fixSpeeds[collisionPairs[i].firstObjectCode].add(firstSpeed);
        fixSpeeds[collisionPairs[i].secondObjectCode].add(secondSpeed);


      }
      else if( firstSpeed.getMagnitude() > 0 ){
        firstBound->updatePositionAndRotation(firstBound->getCenter().add(firstSpeed).getHead());
        fixSpeeds[collisionPairs[i].firstObjectCode].add(firstSpeed);
      }
      else if( secondSpeed.getMagnitude() > 0 ){
        secondBound->updatePositionAndRotation(secondBound->getCenter().add(secondSpeed).getHead());
        fixSpeeds[collisionPairs[i].secondObjectCode].add(secondSpeed);
        int f = fixSpeeds[0].getMagnitude();
        int k = 4;
      }
      else
        break;




    }

    //find collision Normals
    collisionPairs[i].collisionVector = getCollisionNormal(firstBound.get(), secondBound.get());
  }


  



}; 
/*
void BPECollisionWorld::collisionRoutineNormalVectorPhase(vector<BPECollisionData> &collisionPairs){

	


	for(vector<BPECollisionData>::size_type i = 0; i<collisionPairs.size(); i++){
	
		BPEBoundingShape* first = bpeBoundShapes[collisionPairs[i].firstObjectCode].get();		
		BPEBoundingShape* second = bpeBoundShapes[collisionPairs[i].secondObjectCode].get();
		collisionPairs[i].collisionVector = getCollisionNormal(first, second);

	}

	
};
*/
void BPECollisionWorld::updateBoundingBox(const int &code, const BPEPoint3d &center, const BPEVector3d &rotation){
	int size = bpeBoundShapes.size();
	if(size > code){

		//first remove the bounding shape code from the collision area
		bpeAreaCubes[bpeBoundShapes[code]->getXAreaCode()] 
			[bpeBoundShapes[code]->getYAreaCode()] 
			[bpeBoundShapes[code]->getZAreaCode()]->removeCollisionShapeCode(code);

		//update the position and rotation of the bounding shape
		bpeBoundShapes[code]->updatePositionAndRotation(center,rotation);
	
		//find in which collision area is the center of the shape now 
		BPEPoint3d tempPoint = getAreaCode( bpeBoundShapes[code]->getCenter().getHead()	 );

		bpeBoundShapes[code]->setAreaCodes(tempPoint.getX(), tempPoint.getY(), tempPoint.getZ());
		bpeAreaCubes[tempPoint.getX()][tempPoint.getY()][tempPoint.getZ()]->addCollisionShapeCode(code);
	}
	
};

shared_ptr<BPEBoundingShape> BPECollisionWorld::getBoundingBox(const int& objCode){
	return bpeBoundShapes[objCode];
};
//private


BPEVector3d BPECollisionWorld::getCollisionNormal(BPEBoundingShape* first,BPEBoundingShape* second){
	

	bool edgeIsFirsts = false;

	shared_array<BPEVector3d> fVerticesTemp; 
	shared_array<BPEVector3d> sVerticesTemp;
	fVerticesTemp = first->getBoundingBox().getVertices();
	sVerticesTemp = second->getBoundingBox().getVertices();




	vector3df *fVertices = new vector3df[8];
	vector3df *sVertices = new vector3df[8];

	for(int i = 0 ; i < 8; i++){

		fVertices[i] = vector3df(fVerticesTemp[i].getHead().getX(),fVerticesTemp[i].getHead().getY(),fVerticesTemp[i].getHead().getZ());
		sVertices[i] = vector3df(sVerticesTemp[i].getHead().getX(),sVerticesTemp[i].getHead().getY(),sVerticesTemp[i].getHead().getZ());
	}




	BPEPoint3d minEdge(fVertices[0].X,fVertices[0].Y,fVertices[0].Z);
	BPEVector3d fCenterEdgeVec(minEdge,first->getCenter().getHead());
	BPEVector3d sCenterEdgeVec(minEdge,second->getCenter().getHead());
	double minMagnitudes = fCenterEdgeVec.getMagnitude() + sCenterEdgeVec.getMagnitude();
	bool firstBoxEdge = true;

//check the Vertices of the first box
	for(int i = 1; i < 8; i++){
		BPEPoint3d tempMinEdge(fVertices[i].X,fVertices[i].Y,fVertices[i].Z);
		BPEVector3d fCenterEdgeVec(tempMinEdge,first->getCenter().getHead());
		BPEVector3d sCenterEdgeVec(tempMinEdge,second->getCenter().getHead());
		double tempMinMagnitudes = fCenterEdgeVec.getMagnitude() + sCenterEdgeVec.getMagnitude();

		if(tempMinMagnitudes < minMagnitudes){

			minEdge = tempMinEdge;
			minMagnitudes = tempMinMagnitudes;
		}

	}

//check the Vertices of the second box
	for(int i = 0; i < 8; i++){
		BPEPoint3d tempMinEdge(sVertices[i].X,sVertices[i].Y,sVertices[i].Z);
		BPEVector3d fCenterEdgeVec(tempMinEdge,first->getCenter().getHead());
		BPEVector3d sCenterEdgeVec(tempMinEdge,second->getCenter().getHead());
		double tempMinMagnitudes = fCenterEdgeVec.getMagnitude() + sCenterEdgeVec.getMagnitude();

		if(tempMinMagnitudes < minMagnitudes){
			firstBoxEdge = false;
			minEdge = tempMinEdge;
			minMagnitudes = tempMinMagnitudes;

		}
	}


	//create planes and check for intersection with the minimum point
	//if there is intersection with a plane return its normal
	if(firstBoxEdge){
		BPEVector3d connector(minEdge,second->getCenter().getHead());
		BPEPoint3d cHead = connector.getHead();
		BPEPoint3d cTail = connector.getTail();
		queue<plane3d<f32>> sPlanes;

		sPlanes.push(plane3d<f32>(sVertices[0],sVertices[4],sVertices[1]));
		sPlanes.push(plane3d<f32>(sVertices[0],sVertices[2],sVertices[4]));
		sPlanes.push(plane3d<f32>(sVertices[0],sVertices[1],sVertices[2]));
		sPlanes.push(plane3d<f32>(sVertices[7],sVertices[6],sVertices[5]));
		sPlanes.push(plane3d<f32>(sVertices[7],sVertices[5],sVertices[3]));
		sPlanes.push(plane3d<f32>(sVertices[7],sVertices[3],sVertices[2]));

		while(!sPlanes.empty()){
			vector3df intersectionPoint;
			if(sPlanes.front().getIntersectionWithLimitedLine(vector3df(cTail.getX(),cTail.getY(),cTail.getZ()), vector3df(cHead.getX(),cHead.getY(),cHead.getZ()),intersectionPoint)){
				BPEVector3d collisionNormal = BPEVector3d(sPlanes.front().Normal.X,sPlanes.front().Normal.Y,sPlanes.front().Normal.Z);
				//collisionNormal.moveTo(BPEPoint3d(intersectionPoint.X,intersectionPoint.Y,intersectionPoint.Z));
				delete fVertices;
				delete sVertices;
				return collisionNormal;
			}

			sPlanes.pop();
	
		}


	}
	else{
		BPEVector3d connector(minEdge,first->getCenter().getHead());
		BPEPoint3d cHead = connector.getHead();
		BPEPoint3d cTail = connector.getTail();
		queue<plane3d<f32>> fPlanes;
		
		fPlanes.push(plane3d<f32>(fVertices[0],fVertices[4],fVertices[1]));
		fPlanes.push(plane3d<f32>(fVertices[0],fVertices[2],fVertices[4]));
		fPlanes.push(plane3d<f32>(fVertices[0],fVertices[1],fVertices[2]));
		fPlanes.push(plane3d<f32>(fVertices[7],fVertices[6],fVertices[5]));
		fPlanes.push(plane3d<f32>(fVertices[7],fVertices[5],fVertices[3]));
		fPlanes.push(plane3d<f32>(fVertices[7],fVertices[3],fVertices[2]));


		while(!fPlanes.empty()){
			vector3df intersectionPoint;
			if(fPlanes.front().getIntersectionWithLimitedLine(vector3df(cTail.getX(),cTail.getY(),cTail.getZ()), vector3df(cHead.getX(),cHead.getY(),cHead.getZ()),intersectionPoint)){
				BPEVector3d collisionNormal = BPEVector3d(fPlanes.front().Normal.X,fPlanes.front().Normal.Y,fPlanes.front().Normal.Z);
				//collisionNormal.moveTo(BPEPoint3d(intersectionPoint.X,intersectionPoint.Y,intersectionPoint.Z));			
				delete fVertices;
				delete sVertices;
				return collisionNormal;
			}

			fPlanes.pop();
	
		}


	}
	delete fVertices;
	delete sVertices;
	return BPEVector3d(0,1,0);
	
};


void BPECollisionWorld::getCollisionPairs(vector<BPECollisionData> &collPairs){
//needs work immediately
	vector<	shared_ptr<BPEBoundingShape>	>::size_type size = bpeBoundShapes.size();
  
	for(vector<	shared_ptr<BPEBoundingShape>	>::size_type i = 0; i < bpeBoundShapes.size(); i++){

		//false but works for now
		for(vector<	shared_ptr<BPEBoundingShape>	>::size_type j = 0; j  < bpeBoundShapes.size(); j ++){		
			BPEVector3d normal = BPEVector3d();
			double depth=0;
			if(getCollisionResults(bpeBoundShapes[i].get(),bpeBoundShapes[j].get(),normal,depth)){
				int collisionPairsSize = collPairs.size();
				bool willPushBack = true;
				for(int k = 0; k < collisionPairsSize; k++){
					if( (collPairs[k].firstObjectCode == j && collPairs[k].secondObjectCode == i) ){
						
							willPushBack= false;
							break;
					}
				}
				if(willPushBack)
					collPairs.push_back(BPECollisionData(i,j,normal,depth));

			}
			
		}
	}



};
void BPECollisionWorld::recalculateAreaCodes(const vector<bool> &willRecalculate){

  vector<	shared_ptr<BPEBoundingShape>	>::size_type size = bpeBoundShapes.size();
  vector<bool>::size_type tempSize = willRecalculate.size();
	if(size == tempSize)
		for(vector<	shared_ptr<BPEBoundingShape>	>::size_type i = 0; i < size; i++){

			if(willRecalculate[i]){
				BPEPoint3d temp = this->getAreaCode(bpeBoundShapes[i]->getCenter().getHead());
				bpeBoundShapes[i]->setAreaCodes(temp.getX(),temp.getY(),temp.getZ());
			}
		}


};
	

bool BPECollisionWorld::getCollisionResults(BPEBoundingShape *first, BPEBoundingShape *second,BPEVector3d &normal, double &depth){

	if(first == second)
		return false;
	if(!first->collide(second,normal,depth))
		return false;

	return true;
}